/***************************************************************************
 *  Filename: simplecommunicationhandler.cpp
 *  Class: SimpleCommunicationHandler
 *
 *  Copyright 2012  Jacco van der Spek
 *  <jaccovdspek@gmail.com>
 *
 *  Copyright 2012  Mario Henrique Voorsluys
 *  <forcaeluz@gmail.com>
 *
 ****************************************************************************/

/*
 *
 * This file is part of %ProjectName%.
 *
 *    %ProjectName% is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 3 of the License, or
 *    (at your option) any later version.
 *
 *    %ProjectName% is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with %ProjectName%.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "simplecommunicationhandler.h"
#include <QDebug>

SimpleCommunicationHandler::SimpleCommunicationHandler(QObject *parent) :
    QObject(parent)
{
    //Default parrot UART.
    serial.setPortName("/dev/ttyPA0");
    serial.setBaudRate(BAUD19200);
    serial.setParity(PAR_NONE);
    serial.setStopBits(STOP_1);
    if(serial.open(QIODevice::ReadWrite)){
        qDebug() << "[SimpleCommunicationHandler] Serial port open";
    }else{
        qWarning() << "[SimpleCommunicationHandler] Not possible to open port";
        qWarning() << "[SimpleCommunicationHandler] Error: " << serial.errorString();
    }
    connect(&serial, SIGNAL(readyRead()), this, SLOT(getData()));
}

void SimpleCommunicationHandler::getDataA(){
    char buf[256];
    int tag = 0;
    int size = 0;
    int begin = 0;
    float x = 0;
    float y = 0;
    float bearing = 0;
    float altitude = 0;

    int count = serial.read(buf, 256);
    buffer.append(buf, count);
    char tmpchar[4] = {0xFE, 0xFE, 0xFE, 0xFE};
    int location = buffer.indexOf(tmpchar, 0);
    if(location == -1){
        qDebug() << "[SimpleCommunicationHandler] No header found";
        return;
    }
    buffer = buffer.mid(location);
    QByteArray tmp;
    begin += 4;
    tmp = buffer.mid(begin,1);
    begin += 1;
    tag = *reinterpret_cast<quint8*>(tmp.data());
    tmp = buffer.mid(begin, 1);
    begin += 1;
    size = *reinterpret_cast<quint8*>(tmp.data());
    if(buffer.size() < size-begin){
        qDebug() << "[SimpleCommunicationHandler] Message not ready yet";
        return;
    }

    switch(tag){
    case COMMAND_TAKEOFF:
        emit(takeOff());
        qDebug() << "[SimpleCommunicationHandler] Taking off";
        break;
    case COMMAND_LAND:
        emit(land());
        qDebug() << "[SimpleCommunicationHandler] Landing";
        break;
    case COMMAND_TARGET:
        tmp = buffer.mid(begin, 4);
        begin += 4;
        x = *reinterpret_cast<float*>(tmp.data());
        tmp = buffer.mid(begin, 4);
        begin += 4;
        y = *reinterpret_cast<float*>(tmp.data());
        tmp = buffer.mid(begin, 4);
        begin += 4;
        altitude = *reinterpret_cast<int*>(tmp.data());
        emit(relativeTarget(x, y, altitude, 0));
        break;
    case COMMAND_GPSTARG:
        tmp = buffer.mid(begin, 4);
        begin += 4;
        x = *reinterpret_cast<float*>(tmp.data());
        tmp = buffer.mid(begin, 4);
        begin += 4;
        y = *reinterpret_cast<float*>(tmp.data());
        tmp = buffer.mid(begin, 4);
        begin += 4;
        altitude = *reinterpret_cast<int*>(tmp.data());
        emit(gpsTarget(x, y, altitude));
        break;
    case COMMAND_TRIM:
        emit(trim());
        qDebug() << "[SimpleCommunicationHandler] Trimming";
        break;
    case COMMAND_EMERG:
        emit(emergency());
        qDebug() << "[SimpleCommunicationHandler] Emergency Actioned";
        break;
    case DATA_GPS:
        tmp = buffer.mid(1, 4);
        x = *reinterpret_cast<float*>(tmp.data());
        tmp = buffer.mid(5, 4);
        y = *reinterpret_cast<float*>(tmp.data());
        tmp = buffer.mid(9, 4);
        altitude = *reinterpret_cast<int*>(tmp.data());
        tmp = buffer.mid(13, 4);
        bearing = *reinterpret_cast<float*>(tmp.data());
        emit(relativeTarget(x, y, altitude, 0));
        break;
    case DATA_ALTITUDE:
        tmp = buffer.mid(1, 4);
        altitude = *reinterpret_cast<int*>(tmp.data());
        break;
    default:
        qWarning() << "Not a valid usable message";
        buffer.clear();
    }
    buffer = buffer.mid(begin);
}


void SimpleCommunicationHandler::getData(){
    char buf[256];
    int tag = 0;
    float x = 0;
    float y = 0;
    float bearing = 0;
    int altitude = 0;

    int count = serial.read(buf, 255);

    buf[count] = '\0';
    buffer.append(buf);

    if(buffer.size()>=2){
        QByteArray tmp = buffer.mid(0, 1);
        buffer.chop(1);
        tag = *reinterpret_cast<quint8*>(tmp.data());
        switch(tag){
        case COMMAND_TAKEOFF:
            emit(takeOff());
            qDebug() << "[SimpleCommunicationHandler] Taking off";
            break;
        case COMMAND_LAND:
            emit(land());
            qDebug() << "[SimpleCommunicationHandler] Landing";
            break;
        case COMMAND_TARGET:
            tmp = buffer.mid(1, 4);
            x = *reinterpret_cast<float*>(tmp.data());
            tmp = buffer.mid(5, 4);
            y = *reinterpret_cast<float*>(tmp.data());
            tmp = buffer.mid(9, 4);
            altitude = *reinterpret_cast<int*>(tmp.data());
            emit(relativeTarget(x, y, altitude, 0));
            break;
        case COMMAND_GPSTARG:
            tmp = buffer.mid(1, 4);
            x = *reinterpret_cast<float*>(tmp.data());
            tmp = buffer.mid(5, 4);
            y = *reinterpret_cast<float*>(tmp.data());
            tmp = buffer.mid(9, 4);
            altitude = *reinterpret_cast<int*>(tmp.data());
            emit(gpsTarget(x, y, altitude));
            break;
        case COMMAND_TRIM:
            emit(trim());
            break;
        case COMMAND_EMERG:
            emit(emergency());
            break;
        case DATA_GPS:
            tmp = buffer.mid(1, 4);
            x = *reinterpret_cast<float*>(tmp.data());
            tmp = buffer.mid(5, 4);
            y = *reinterpret_cast<float*>(tmp.data());
            tmp = buffer.mid(9, 4);
            altitude = *reinterpret_cast<int*>(tmp.data());
            tmp = buffer.mid(13, 4);
            bearing = *reinterpret_cast<float*>(tmp.data());
            emit(relativeTarget(x, y, altitude, 0));
            break;
        case DATA_ALTITUDE:
            tmp = buffer.mid(1, 4);
            altitude = *reinterpret_cast<int*>(tmp.data());
            break;
        default:
            qWarning() << "Not a valid usable message";
            buffer.clear();
        }
    }
}

void SimpleCommunicationHandler::sendAltitude(int altitude){
    quint32 header = HEADER;
    quint8 tag = DATA_ALTITUDE;
    quint8 size = 10;
    QByteArray data;
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&tag)), 4));
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&tag)), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&size), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&altitude), 4));
    sendData(data);
}

void SimpleCommunicationHandler::sendBattery(int perc){
    quint32 header = HEADER;
    quint8 tag = DATA_BAT;
    quint8 size = 10;
    QByteArray data;
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&header)), 4));
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&tag)), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&size), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&perc), 4));
    sendData(data);
}

void SimpleCommunicationHandler::sendGyros(float roll, float pitch, float yaw){
    quint32 header = HEADER;
    quint8 tag = DATA_GYR;
    quint8 size = 18;
    QByteArray data;
    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&header)), 4));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&tag), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&size), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&roll), 4));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&pitch), 4));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&yaw), 4));
    sendData(data);

}

void SimpleCommunicationHandler::sendVelocity(float vx, float vy, float vz){
    quint32 header = HEADER;
    quint8 tag = DATA_VEL;
    quint8 size = 18;
    QByteArray data;

    data.append(QByteArray::fromRawData((reinterpret_cast<char*>(&header)), 4));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&tag), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&size), 1));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&vx), 4));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&vy), 4));
    data.append(QByteArray::fromRawData(reinterpret_cast<char*>(&vz), 4));
    sendData(data);
}

void SimpleCommunicationHandler::sendData(QByteArray data){
    serial.write(data);
    serial.flush();
    qDebug() << "[SimpleCommunicationHandler] Data sent: " << data.toHex();
}
